/**********************************************************************************************************************
 *
 * Project:    mGameEngine
 * File:       matrix3x3.h
 * Author:     karooolek
 * Created on: 2009-03-04
 *
 **********************************************************************************************************************/

#ifndef MATRIX3X3_H_
#define MATRIX3X3_H_

#include "vector3.h"
#include "quaternion.h"
#include <cstring>
#include <algorithm>

namespace mGameEngine
{

/**
 * Three by three floats matrix.
 */
struct Matrix3x3
{
    /**
     * Matrix components
     */
    union
    {
        float m[3][3];
        float _m[9];
    };

    /**
     * Create matrix with initial value on diagonal.
     * @param a diagonal value.
     */
    Matrix3x3(float a = 1.0f)
    {
        _m[0] = a;    _m[3] = 0.0f; _m[6] = 0.0f;
        _m[1] = 0.0f; _m[4] = a;    _m[7] = 0.0f;
        _m[2] = 0.0f; _m[5] = 0.0f; _m[8] = a;
    }

    /**
     * Create matrix with initial values.
     * @param m00 first column, first row
     * @param m10 second column, first row
     * @param m20 third column, first row
     * @param m01 first column, second row
     * @param m11 second column, second row
     * @param m21 third column, second row
     * @param m02 first column, third row
     * @param m12 second column, third row
     * @param m22 third column, third row
     */
    Matrix3x3(float m00, float m10, float m20,
              float m01, float m11, float m21,
              float m02, float m12, float m22)
    {
        _m[0] = m00; _m[3] = m10; _m[6] = m20;
        _m[1] = m01; _m[4] = m11; _m[7] = m21;
        _m[2] = m02; _m[5] = m12; _m[8] = m22;
    }

    /**
     * Create matrix from 2x2 sub-matrix.
     * @param m2 sub-matrix to copy.
     */
    Matrix3x3(const Matrix2x2 &m2)
    {
        operator=(m2);
    }

    /**
     * Copy constructor.
     * @param m2 matrix to copy.
     */
    Matrix3x3(const Matrix3x3 &m2)
    {
        operator=(m2);
    }

    /**
     * Create matrix with initial values from three vectors.
     * @param v0 first column
     * @param v1 second column
     * @param v2 third column
     */
    Matrix3x3(const Vector3 &v0,
              const Vector3 &v1,
              const Vector3 &v2)
    {
        _m[0] = v0.x; _m[3] = v1.x; _m[6] = v2.x;
        _m[1] = v0.y; _m[4] = v1.y; _m[7] = v2.y;
        _m[2] = v0.z; _m[5] = v1.z; _m[8] = v2.z;
    }

    /**
     * Create matrix with initial values from values array.
     * @param v nine floats array
     */
    Matrix3x3(const float *v)
    {
        memcpy(_m, v, 9 * sizeof(float));
    }

    /**
     * Create matrix with initial orientation.
     * @param q orientation quaternion.
     */
    Matrix3x3(const Quaternion &q)
    {
        float xx(q.x*q.x), yy(q.y*q.y), zz(q.z*q.z);
        float xy(q.x*q.y), xz(q.x*q.z), yz(q.y*q.z), wx(q.w*q.x), wy(q.w*q.y), wz(q.w*q.z);

        _m[0] = 1.0f - 2.0f * (yy + zz);
        _m[1] = 2.0f * (xy + wz);
        _m[2] = 2.0f * (xz - wy);

        _m[3] = 2.0f * (xy - wz);
        _m[4] = 1.0f - 2.0f * (xx + zz);
        _m[5] = 2.0f * (yz + wx);

        _m[6] = 2.0f * (xz + wy);
        _m[7] = 2.0f * (yz - wx);
        _m[8] = 1.0f - 2.0f * (xx + yy);
    }

    /**
     * Assign 2x2 sub-matrix.
     * @param m2 matrix to assign.
     * @return matrix with assigned 2x2 values. Rest remains unchanged.
     */
    Matrix3x3 &operator=(const Matrix2x2 &m2)
    {
        _m[0] = m2._m[0]; _m[3] = m2._m[2];
        _m[1] = m2._m[1]; _m[4] = m2._m[3];
        return *this;
    }

    /**
     * Assign matrix.
     * @param m2 matrix to assign.
     * @return matrix with assigned value.
     */
    Matrix3x3 &operator=(const Matrix3x3 &m2)
    {
        memcpy(_m, m2._m, 9 * sizeof(float));
        return *this;
    }

    /**
     * Add matrix.
     * @param m2 matrix to add
     * @return sum of matrices
     */
    Matrix3x3 &operator+=(const Matrix3x3 &m2)
    {
        _m[0] += m2._m[0]; _m[3] += m2._m[3]; _m[6] += m2._m[6];
        _m[1] += m2._m[1]; _m[4] += m2._m[4]; _m[7] += m2._m[7];
        _m[2] += m2._m[2]; _m[5] += m2._m[5]; _m[8] += m2._m[8];
        return *this;
    }

    /**
     * Add matrices.
     * @param m2 matrix to add
     * @return sum of matrices
     */
    Matrix3x3 operator+(const Matrix3x3 &m2) const
    {
        return Matrix3x3(_m[0] + m2._m[0], _m[3] + m2._m[3], _m[6] + m2._m[6],
                         _m[1] + m2._m[1], _m[4] + m2._m[4], _m[7] + m2._m[7],
                         _m[2] + m2._m[2], _m[5] + m2._m[5], _m[8] + m2._m[8]);
    }

    /**
     * Negate matrix.
     * @return negated matrix
     */
    Matrix3x3 operator-() const
    {
        return *this * -1.0f;
    }

    /**
     * Subtract matrix.
     * @param m2 matrix to subtract
     * @return difference of matrices
     */
    Matrix3x3 &operator-=(const Matrix3x3 &m2)
    {
        _m[0] -= m2._m[0]; _m[3] -= m2._m[3]; _m[6] -= m2._m[6];
        _m[1] -= m2._m[1]; _m[4] -= m2._m[4]; _m[7] -= m2._m[7];
        _m[2] -= m2._m[2]; _m[5] -= m2._m[5]; _m[8] -= m2._m[8];
        return *this;
    }

    /**
     * Subtract matrices.
     * @param m2 matrix to subtract
     * @return difference of matrices
     */
    Matrix3x3 operator-(const Matrix3x3 &m2) const
    {
        return Matrix3x3(_m[0] - m2._m[0], _m[3] - m2._m[3], _m[6] - m2._m[6],
                         _m[1] - m2._m[1], _m[4] - m2._m[4], _m[7] - m2._m[7],
                         _m[2] - m2._m[2], _m[5] - m2._m[5], _m[8] - m2._m[8]);
    }

    /**
     * Multiply matrix.
     * @param m2 matrix multilplier
     * @return multiplied matrix
     */
    Matrix3x3 &operator*=(const Matrix3x3 &m2)
    {
        float m00 = _m[0], m10 = _m[3];
        float m01 = _m[1], m11 = _m[4];
        float m02 = _m[2], m12 = _m[5];

        _m[0] = m00 * m2._m[0] + m10 * m2._m[1] + _m[6] * m2._m[2];
        _m[3] = m00 * m2._m[3] + m10 * m2._m[4] + _m[6] * m2._m[5];
        _m[6] = m00 * m2._m[6] + m10 * m2._m[7] + _m[6] * m2._m[8];

        _m[1] = m01 * m2._m[0] + m11 * m2._m[1] + _m[7] * m2._m[2];
        _m[4] = m01 * m2._m[3] + m11 * m2._m[4] + _m[7] * m2._m[5];
        _m[7] = m01 * m2._m[6] + m11 * m2._m[7] + _m[7] * m2._m[8];

        _m[2] = m02 * m2._m[0] + m12 * m2._m[1] + _m[8] * m2._m[2];
        _m[5] = m02 * m2._m[3] + m12 * m2._m[4] + _m[8] * m2._m[5];
        _m[8] = m02 * m2._m[6] + m12 * m2._m[7] + _m[8] * m2._m[8];

        return *this;
    }

    /**
     * Multiply matrices.
     * @param m2 matrix multiplier
     * @return multiplied matrices
     */
    Matrix3x3 operator*(const Matrix3x3 &m2) const
    {
        return Matrix3x3(_m[0] * m2._m[0] + _m[3] * m2._m[1] + _m[6] * m2._m[2],
                         _m[0] * m2._m[3] + _m[3] * m2._m[4] + _m[6] * m2._m[5],
                         _m[0] * m2._m[6] + _m[3] * m2._m[7] + _m[6] * m2._m[8],

                         _m[1] * m2._m[0] + _m[4] * m2._m[1] + _m[7] * m2._m[2],
                         _m[1] * m2._m[3] + _m[4] * m2._m[4] + _m[7] * m2._m[5],
                         _m[1] * m2._m[6] + _m[4] * m2._m[7] + _m[7] * m2._m[8],

                         _m[2] * m2._m[0] + _m[5] * m2._m[1] + _m[8] * m2._m[2],
                         _m[2] * m2._m[3] + _m[5] * m2._m[4] + _m[8] * m2._m[5],
                         _m[2] * m2._m[6] + _m[5] * m2._m[7] + _m[8] * m2._m[8]);
    }

    /**
     * Multiply by vector.
     * @param v vector multiplier
     * @return multiplied matrix - a vector
     */
    Vector2 operator*(const Vector2 &v) const
    {
        return Vector3(
            _m[0] * v.x + _m[3] * v.y + _m[6],
            _m[1] * v.x + _m[4] * v.y + _m[7]);
    }

    /**
     * Multiply by vector.
     * @param v vector multiplier
     * @return multiplied matrix - a vector
     */
    Vector3 operator*(const Vector3 &v) const
    {
        return Vector3(_m[0] * v.x + _m[3] * v.y + _m[6] * v.z,
                       _m[1] * v.x + _m[4] * v.y + _m[7] * v.z,
                       _m[2] * v.x + _m[5] * v.y + _m[8] * v.z);
    }

    /**
     * Multiply by scalar.
     * @param s scalar mutliplier
     * @return multiplied matrix
     */
    Matrix3x3 &operator*=(float s)
    {
        _m[0] *= s; _m[3] *= s; _m[6] *= s;
        _m[1] *= s; _m[4] *= s; _m[7] *= s;
        _m[2] *= s; _m[5] *= s; _m[8] *= s;

        return *this;
    }

    /**
     * Multiply by scalar.
     * @param s scalar multiplier
     * @return multiplied matrix
     */
    Matrix3x3 operator*(float s) const
    {
        return Matrix3x3(_m[0] * s, _m[3] * s, _m[6] * s,
                         _m[1] * s, _m[4] * s, _m[7] * s,
                         _m[2] * s, _m[5] * s, _m[8] * s);
    }

    /**
     * Divide by scalar.
     * @param s scalar divisor
     * @return divided matrix
     */
    Matrix3x3 &operator/=(float s)
    {
        s = s ? 1.0f/s : 1.0f;

        _m[0] *= s; _m[3] *= s; _m[6] *= s;
        _m[1] *= s; _m[4] *= s; _m[7] *= s;
        _m[2] *= s; _m[5] *= s; _m[8] *= s;

        return *this;
    }

    /**
     * Divide by scalar.
     * @param s scalar divisor
     * @return divided matrix
     */
    Matrix3x3 operator/(float s) const
    {
        s = s ? 1.0f/s : 1.0f;

        return Matrix3x3(_m[0] * s, _m[3] * s, _m[6] * s,
                         _m[1] * s, _m[4] * s, _m[7] * s,
                         _m[2] * s, _m[5] * s, _m[8] * s);
    }

    /**
     * Matrices equal.
     * @param m2 matrix to compare
     * @return true if all components equal
     */
    bool operator==(const Matrix3x3 &m2) const
    {
        return
        (
            _m[0] == m2._m[0] && _m[3] == m2._m[3] && _m[6] == m2._m[6] &&
            _m[1] == m2._m[1] && _m[4] == m2._m[4] && _m[7] == m2._m[7] &&
            _m[2] == m2._m[2] && _m[5] == m2._m[5] && _m[8] == m2._m[8]
        );
    }

    /**
     * Matrices differ.
     * @param m2 matrix to compare
     * @return true if any of components differs
     */
    bool operator!=(const Matrix3x3 &m2) const
    {
        return
        (
            _m[0] != m2._m[0] || _m[3] != m2._m[3] || _m[6] != m2._m[6] ||
            _m[1] != m2._m[1] || _m[4] != m2._m[4] || _m[7] != m2._m[7] ||
            _m[2] != m2._m[2] || _m[5] != m2._m[5] || _m[8] != m2._m[8]
        );
    }

    /**
     * Floats array cast.
     * @return floats array
     */
    operator float *()
    {
        return _m;
    }

    /**
     * Floats const array cast.
     * @return floats const array
     */
    operator const float *() const
    {
        return _m;
    }

    /**
     * 2x2 sub-matrix cast.
     * @return upper left 2x2 sub-matrix.
     */
    operator Matrix2x2() const
    {
        return Matrix2x2(_m[0], _m[3],
                         _m[1], _m[4]);
    }

    /**
     * Quaternion cast.
     * @return quaternion created from matrix orientation.
     */
    operator Quaternion() const
    {
        float tr = _m[0] + _m[4] + _m[8];

        if(tr > 0.0f)
        {
            float s = 0.5f / sqrtf(tr + 1.0f);

            return Quaternion((_m[5] - _m[7]) * s, (_m[6] - _m[2]) * s, (_m[1] - _m[3]) * s, 0.25f / s);
        }
        else if(_m[0] > _m[4] && _m[0] > _m[8])
        {
            float s = 2.0f * sqrtf(1.0f + _m[0] - _m[4] - _m[8]);

            return Quaternion(-0.25f * s, (-_m[1] - _m[3]) / s, (-_m[2] - _m[6]) / s, (_m[7] - _m[5]) / s);
        }
        else if(_m[4] > _m[8])
        {
            float s = 2.0f * sqrtf(1.0f + _m[4] - _m[0] - _m[8]);

            return Quaternion((-_m[1] - _m[3]) / s, -0.25f * s, (-_m[5] - _m[7]) / s, (_m[2] - _m[6]) / s);
        }
        else
        {
            float s = 2.0f * sqrtf(1.0f + _m[8] - _m[0] - _m[4]);

            return Quaternion((-_m[2] - _m[6]) / s, (-_m[5] - _m[7]) / s, -0.25f * s, (_m[3] - _m[1]) / s);
        }
    }

    /**
     * Transpose matrix.
     * @return transposed matrix.
     */
    Matrix3x3 &transpose()
    {
        std::swap(_m[1], _m[3]);
        std::swap(_m[2], _m[6]);
        std::swap(_m[5], _m[7]);

        return *this;
    }

    /**
     * Invert matrix.
     * @return inverted matrix.
     */
    Matrix3x3 &invert()
    {
        return transpose();

//        // matrix is orthogonal
//        if(Matrix3x3(*this) * transpose() == Matrix3x3())
//        {
//            return *this;
//        }
//        // no transpose it back
//        else
//        {
//            transpose();
//        }
//
//        // TODO invert matrix
//
//        return *this;
    }

    /**
     * Spherical linear interpolation between two matrices.
     * @param m1 first matrix
     * @param m2 second matrix
     * @param t interpolation factor.
     * @return spherically linearly interpolated matrix between m1 and m2.
     */
    friend Matrix3x3 slerp(const Matrix3x3 &m1, const Matrix3x3 &m2, float t)
    {
        return Matrix3x3(slerp(Vector3(m1.m[0]), Vector3(m2.m[0]), t),
                         slerp(Vector3(m1.m[1]), Vector3(m2.m[1]), t),
                         slerp(Vector3(m1.m[2]), Vector3(m2.m[2]), t));
    }
};

}

#endif // MATRIX3X3_H_
